/*******************************************************************************
* Copyright 2017 CLNST CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

#ifndef ___S6H4D_CTRL_H__
#define ___S6H4D_CTRL_H__

#include <stdio.h>
#include <fcntl.h>
#include <string.h>
#include <stdlib.h>
#include <sys/times.h>
#include <sys/types.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>

#include <serial/serial.h> 

#define JOINT_MAX   6
#define PI                        3.1415926

#define INIT_J1    0.12
#define INIT_J2    -17.99
#define INIT_J3    78.72
#define INIT_J4    0
#define INIT_J5    44.99
#define INIT_J6    -0.03

#define POS_X           0
#define POS_Y           1
#define POS_Z           2

#define J1                  0
#define J2                  1
#define J3                  2
#define J4                  3
#define J5                  4
#define J6                  5

#define S6H4D_RESET       0
#define S6H4D_PLAY          1
#define S6H4D_STOP         2

int s6h4d_chkReset(const double dt[6]);
int s6h4d_txSig(unsigned char cmd, const double dt[6]);
int s6h4d_rxSig(double dt[6], double ps[3]);
int s6h4d_init(const char * dev);
void s6h4d_deinit(void);

#endif // ___S6H4D_CTRL_H__
